﻿/**
 * MOTOR 2 - RAPID FLASH DYNAMICS
 * Copyright (c) 2006-2008 Michael Baczynski http://www.polygonal.de
 * 
 * Based on Box2D by Erin Catto, http://www.box2d.org
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
 * arising from the use of this software.
 * 
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 * 
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */
package de.polygonal.motor2.dynamics {
	import flash.events.EventDispatcher;
	import de.polygonal.motor2.World;
	import de.polygonal.motor2.collision.shapes.ShapeSkeleton;
	import de.polygonal.motor2.collision.shapes.data.ShapeData;
	import de.polygonal.motor2.dynamics.RigidBodyData;
	import de.polygonal.motor2.dynamics.contact.ContactNode;
	import de.polygonal.motor2.math.V2;	

	/**
	 * A rigid body.
	 * 
	 * Internal computations are done in term of the center of mass position.
	 * The center of mass may be offset from the body's origin.
	 */
	public class RigidBody extends EventDispatcher {

		public static const k_bitStatic:int = 0x01;
		public static const k_bitFrozen:int = 0x02;
		public static const k_bitSleep:int = 0x04;
		public static const k_bitAllowSleep:int = 0x08;
		public static const k_bitDestroy:int = 0x10;
		public static const k_bitIsland:int = 0x20;
		public static const k_bitExclude:int = k_bitStatic | k_bitIsland | k_bitSleep | k_bitFrozen;
		public var id:int;
		public var next:RigidBody;
		public var prev:RigidBody;
		public var x:Number, vx:Number, fx:Number;
		public var y:Number, vy:Number, fy:Number;
		public var r:Number, w:Number, t:Number;
		/* mass & inertia */
		public var mass:Number;
		public var invMass:Number;
		public var I:Number;
		public var invI:Number;
		/* 2x2 column rotation matrix */
		public var r11:Number, r12:Number;
		public var r21:Number, r22:Number;
		/* local vector from client origin to cm */
		public var cx:Number, cy:Number;
		public var linDamping:Number;
		public var angDamping:Number;
		public var shapeCount:int;
		public var world:World;
		public var shapeList:ShapeSkeleton;
		//		public var jointList:JointNode;
		public var contactList:ContactNode;
		public var stateBits:int;
		public var sleepTime:Number;

		public function RigidBody(world:World, data:RigidBodyData) {
			init(world, data);
		}

		public function deconstruct():void {
			prev = next = null;
			var s:ShapeSkeleton = shapeList;
			while (s) {
				var s0:ShapeSkeleton = s;
				s = s.next;
				s0.deconstruct();
			}
		}

		public function getCenter(out:V2):void {
			out.x = x;
			out.y = y;
		}

		public function setCenter(x:Number, y:Number, rot:Number):void {
			if ((stateBits & k_bitFrozen) == 0) {
				r = rot;
				var c:Number = Math.cos(r);
				var s:Number = Math.sin(r);
				r11 = c; 
				r12 = -s;
				r21 = s; 
				r22 = c;
				
				this.x = x;
				this.y = y;
				
				var shape:ShapeSkeleton = shapeList;
				while (shape) {
					shape.update();
					shape = shape.next;
				}
				//TODO flush broadphase
			}
		}

		public function getOrigin(out:V2):void {
			out.x = x - (r11 * cx + r12 * cy);
			out.y = y - (r21 * cx + r22 * cy);
		}

		public function setOrigin(x:Number, y:Number, rot:Number):void {
			if ((stateBits & k_bitFrozen) == 0) {
				r = rot;
				var c:Number = Math.cos(r);
				var s:Number = Math.sin(r);
				r11 = c; 
				r12 = -s;
				r21 = s; 
				r22 = c;
				
				this.x = x + (r11 * cx + r12 * cy);
				this.y = y + (r21 * cx + r22 * cy);
				
				var shape:ShapeSkeleton = shapeList;
				while (shape) {
					shape.update();
					shape = shape.next;
				}
				
				//TODO reset broadphase
				//m_world.m_broadPhase.Flush();
			}
		}

		public function rotate(deg:Number):void {
			//wrap input angle to 0..2PI
			if (deg < 0)
				deg += 360;
			else
			if (deg > 360)
				deg -= 360;
			
			r = deg * (Math.PI / 180);
			
			updateShapes();
		}

		public function applyForce(fx:Number, fy:Number):void {
			if ((stateBits & k_bitSleep) == 0) {
				this.fx += fx;
				this.fy += fy;
			}
		}

		public function applyForceAt(fx:Number, fy:Number, px:Number, py:Number):void {
			if ((stateBits & k_bitSleep) == 0) {
				this.fx += fx;
				this.fy += fy;
				t += (px - x) * fy - (py - y) * fx;
			}
		}

		public function applyTorque(torque:Number):void {
			if ((stateBits & k_bitSleep) == 0)
				t += torque;
		}

		public function applyImpulse(ix:Number, iy:Number):void {
			if ((stateBits & k_bitSleep) == 0) {
				vx += invMass * ix;
				vy += invMass * iy;
			}
		}

		public function applyImpulseAt(ix:Number, iy:Number, px:Number, py:Number):void {
			if ((stateBits & k_bitSleep) == 0) {
				vx += invMass * ix;
				vy += invMass * iy;
				w += invI * (px - x) * iy - (py - y) * ix;
			}
		}

		public function getWorldSpacePoint(p:V2, out:V2):void {
			//Pworld = RT * Plocal
			out.x = x + (r11 * p.x + r12 * p.y);
			out.y = y + (r21 * p.x + r22 * p.y);
		}

		public function getWorldSpaceDirection(d:V2, out:V2):void {
			out.x = r11 * d.x + r12 * d.y;
			out.y = r21 * d.x + r22 * d.y;
		}

		public function getLocalSpacePoint(p:V2, out:V2):void {
			//Plocal = RT * (Pworld - P)
			out.x = r11 * (p.x - x) + r21 * (p.y - y);
			out.y = r12 * (p.x - x) + r22 * (p.y - y);
		}

		public function getLocalSpaceDirection(d:V2, out:V2):void {
			out.x = r11 * d.x + r21 * d.y;
			out.y = r12 * d.x + r22 * d.y;
		}

		public function isStatic():Boolean {
			return (stateBits & k_bitStatic) == k_bitStatic;
		}

		public function isFrozen():Boolean {
			return (stateBits & k_bitFrozen) == k_bitFrozen;
		}

		public function isSleeping():Boolean {
			return (stateBits & k_bitSleep) == k_bitSleep;
		}

		public function allowSleeping(flag:Boolean):void {
			if (flag)
				stateBits |= k_bitAllowSleep;
			else {
				stateBits &= ~k_bitAllowSleep;
				wakeUp();
			}
		}

		public function wakeUp():void {
			stateBits &= ~k_bitSleep;
			sleepTime = 0;
		}

		public function updateShapes():void {
			var c:Number, s:Number;
			c = Math.cos(r);
			s = Math.sin(r);
			r11 = c; 
			r12 = -s;
			r21 = s; 
			r22 = c;
			
			var sd:ShapeSkeleton = shapeList;
			while (sd) {
				sd.update();
				sd = sd.next;
			}
		}

		// This is used to prevent connected bodies from colliding.
		// It may lie, depending on the collideConnected flag.
		public function isConnected(other:RigidBody):Boolean {
			//for (b2JointNode* jn = m_jointList; jn; jn = jn.next)
			//{
			//if (jn.other == other)
			//return jn.joint.m_collideConnected == false;
			//}
			return false;
		}

		/**
		 * invoked when the child shape has no proxy.
		 */
		public function freeze():void {
			stateBits |= k_bitFrozen;
			vx = vy = w = 0;
		}

		private function init(world:World, bd:RigidBodyData):void {
			var c:Number, s:Number, shapeMass:Number;
			var rx:Number, ry:Number;
			var sd:ShapeData, shape:ShapeSkeleton;
			var cm:V2;
			var shapeClass:Class;
			
			this.world = world;
			
			x = bd.x;
			y = bd.y;
			r = bd.r;
			
			c = Math.cos(r);
			s = Math.sin(r);
			
			r11 = c; 
			r12 = -s;
			r21 = s; 
			r22 = c;
			
			vx = vy = w = 0;
			fx = fy = t = 0;
			
			mass = invMass = I = invI = 0;
			
			cx = cy = 0;
			
			linDamping = 1 - bd.linDamping;
			linDamping = (linDamping < 0) ? 0 : (linDamping > 1) ? 1 : linDamping;
			
			angDamping = 1 - bd.angDamping;
			angDamping = (angDamping < 0) ? 0 : (angDamping > 1) ? 1 : angDamping;
			
			shapeCount = 0;
			stateBits = 0;
			sleepTime = 0;
			
			if (bd.allowSleep) stateBits |= k_bitAllowSleep;
			if (bd.isSleeping) stateBits |= k_bitSleep;
			
			//			jointList   = null;
			contactList = null;
			next = prev = null;
			
			sd = bd.shapeDataList;
			while (sd) {
				shapeMass = sd.getMass();
				cm = sd.getCM();
				
				mass += shapeMass;
				cx += shapeMass * (sd.mx + cm.x);
				cy += shapeMass * (sd.my + cm.y);
				
				shapeCount++;
				sd = sd.next;
			}
			
			//compute cm & shift origin to cm.
			if (mass > 0) {
				cx /= mass;
				cy /= mass;
				
				x += r11 * cx + r12 * cy;
				y += r21 * cx + r22 * cy;
			}
			else
				stateBits |= k_bitStatic;
			
			//compute total moment of inertia
			if (!bd.preventRotation) {
				sd = bd.shapeDataList;
				while (sd) {
					I += sd.getInertia();
					cm = sd.getCM();
					
					rx = sd.mx + cm.x - cx;
					ry = sd.my + cm.y - cy;
					I += sd.getMass() * (rx * rx + ry * ry);
					
					sd = sd.next;
				}
				if (I > 0) invI = 1 / I;
			}
			
			invMass = (mass > 0) ? (1 / mass) : 0;
			
			//compute cm velocity
			if (!bd.isSleeping && invMass > 0) {
				vx = bd.vx + -bd.w * cy;
				vy = bd.vy + bd.w * cy;
				w = bd.w;
			}
			
			//create all shapes
			sd = bd.shapeDataList;
			while (sd) {
				shapeClass = sd.getShapeClass();
				shape = new shapeClass(sd, this);
				shape.next = shapeList;
				shapeList = shape;
				sd = sd.next;
			}
		}
	}
}